#include "main.h"
#include "motor_encoder.h"
#include "motor_gpio.h"
#include "pid_controller.h"
#include "motor_pwm.h"
#include "timer.h"
#include "mq_wrapper.h"

// 记录运行次数
int current_time = 0;
int duration_in_tick = 100; // 运行次数:100 * 20ms = 2s
pthread_mutex_t running_mutex = PTHREAD_MUTEX_INITIALIZER;
char output_file_name[50] = "default_data_output.txt";
const char *horizontal_line = "------------------------------------------";
int running = 0;

// 控制线程的执行函数：设置为实时线程后，进入可唤醒休眠。
// 控制线程是四个线程里，唯一可以接收SIGALRM信号的线程。
void *control_thread_func(void *arg)
{
    static char data_buffer[100];
    while (1)
    {
        pause();
        if (running)
        {
            // 获取电机转速
            double speed_rpm = get_and_clear_encoder_edge_count() * 1.0 / 4.0 / 330.0 / SAMPLE_PERIOD * 1000.0 * 1000.0 * 60.0;
            // 给PID控制器传入转速参数，得到输出占空比
            double duty_cycle = pid_controller(speed_rpm);
            // 设置输出PWM的占空比
            set_pwm_output(duty_cycle);
            // 将时间、输出占空比、电机转速写入缓冲区，然后通过消息队列发送给数据记录进程
            sprintf(data_buffer, "%f %f %f\n", (current_time + 1) * (SAMPLE_PERIOD / 1000.0 / 1000.0), duty_cycle, speed_rpm);
            int ret = mq_wrapper_send_data(data_buffer);
            if (ret == -1)
                printf("发送消息“%s”失败\n", data_buffer);
            // 若达到了设定的运行次数，则向数据记录进程发送"close"命令，让数据记录进程关闭输出文件。并将PWM输出设置为0（停止电机）后退出程序。
            if (++current_time > duration_in_tick - 1)
            {
                pthread_mutex_lock(&running_mutex);
                running = 0;
                pthread_mutex_unlock(&running_mutex);
            }
        }
        else
        {
            while (mq_wrapper_send_close_file_cmd() == -1)
                ;
            // mq_send(motor_data_queue, close_file_cmd, strlen(close_file_cmd) + 1, 20);
            set_pwm_output(0);
            stop_timer();
        }
    }
    sleep(20000);
}

void print_params()
{
    puts(horizontal_line);
    if (open_loop)
        printf("开环控制, 开环PWM占空比:%f%%\n", open_loop_duty_cycle);
    else
        puts("闭环控制");
    printf("电机转向:%d(0正1反)\n", direction);
    printf("最大运行时间:%dms\n", duration_in_tick * SAMPLE_PERIOD / 1000);
    printf("输出文件:%s\n", output_file_name);
    printf("Kp: %f, Ki: %f, Kd: %f\n", Kp, Ki, Kd);
    printf("目标速度: %f\n", target_speed);
    puts(horizontal_line);
}

int parse_and_exec_user_input_cmd(const char *cmd)
{
    // printf("输入指令:%s\n", cmd);
    if (strcmp(cmd, "stop") == 0)
    {
        pthread_mutex_lock(&running_mutex);
        running = 0;
        pthread_mutex_unlock(&running_mutex);
        return 0;
    }
    else if (strcmp(cmd, "param") == 0)
    {
        print_params();
        return 0;
    }
    if (running)
    {
        printf("电机运行中，无法执行该命令\n");
        return -1;
    }
    if (strncmp(cmd, "ol:", 3) == 0)
    {
        open_loop = atoi(cmd + 3);
        return 0;
    }
    else if (strncmp(cmd, "old:", 4) == 0)
    {
        open_loop_duty_cycle = strtod(cmd + 4, NULL);
        return 0;
    }
    else if (strncmp(cmd, "dir:", 4) == 0)
    {
        set_motor_direction(atoi(cmd + 4));
        return 0;
    }
    else if (strncmp(cmd, "dur:", 4) == 0)
    {
        duration_in_tick = strtod(cmd + 4, NULL) / SAMPLE_PERIOD * 1000.0;
        return 0;
    }
    else if (strncmp(cmd, "file:", 5) == 0)
    {
        strcpy(output_file_name, cmd + 5);
        return 0;
    }
    else if (strncmp(cmd, "p:", 2) == 0)
    {
        Kp = strtod(cmd + 2, NULL);
        return 0;
    }
    else if (strncmp(cmd, "i:", 2) == 0)
    {
        Ki = strtod(cmd + 2, NULL);
        return 0;
    }
    else if (strncmp(cmd, "d:", 2) == 0)
    {
        Kd = strtod(cmd + 2, NULL);
        return 0;
    }
    else if (strncmp(cmd, "target:", 7) == 0)
    {
        target_speed = strtod(cmd + 7, NULL);
        return 0;
    }
    else if (strncmp(cmd, "start", 5) == 0)
    {
        current_time = 0;
        running = 1;
        get_and_clear_encoder_edge_count();
        reset_controller_state();
        mq_wrapper_send_open_file_cmd(output_file_name);
        start_timer(SAMPLE_PERIOD);
        return 0;
    }
    else
    {
        puts("未知指令");
        return -1;
    }
}

int main(int argc, char *argv[])
{
    pthread_t control_thread;
    // 初始化消息队列
    mq_wrapper_init();
    // 锁定内存
    if (mlockall(MCL_CURRENT | MCL_CURRENT) == -1)
    {
        perror("mlockall");
        exit(-1);
    }
    // 初始化GPIO并设置电机转向
    init_gpio();
    set_motor_direction(0);
    // 初始化PWM
    init_pwm();

    // 设置PID控制器的参数，包括目标速度
    set_target_speed(220);
    Kp = 0.197282666954605;
    Ki = 3.63388243633273;
    Kd = 0.00267760799658669;
    // 创建控制线程
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    struct sched_param param = {.sched_priority = 99};
    pthread_attr_setschedparam(&attr, &param);
    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    pthread_create(&control_thread, &attr, control_thread_func, NULL);
    // 创建编码器输入边缘的读取线程
    init_encoder_poll_threads();
    // 主线程应当屏蔽定时器的SIGALRM信号，让控制线程运行信号处理函数
    mask_sig_alarm();
    printf("初始化完毕\n");
    char cmd_buf[50];
    while (1)
    {
        // 获取一行用户的指令
        fgets(cmd_buf, sizeof(cmd_buf), stdin);
        // 去掉换行符
        size_t length = strlen(cmd_buf);
        if (length > 0 && cmd_buf[length - 1] == '\n')
            cmd_buf[length - 1] = '\0';
        parse_and_exec_user_input_cmd(cmd_buf);
    }
    return 0;
}
